
#ifndef NAV_DWB_CRITICS__PREFER_FORWARD_HPP_
#define NAV_DWB_CRITICS__PREFER_FORWARD_HPP_

#include <string>
#include "nav_dwb_controller_pkg/trajectory_critic.hpp"

namespace nav_dwb_critics
{

/**
 * @class PreferForwardCritic
 * @brief Penalize trajectories with move backwards and/or turn too much
 *
 * Has three different scoring conditions:
 * 1) If the trajectory's x velocity is negative, return the penalty
 * 2) If the trajectory's x is low and the theta is also low, return the penalty.
 * 3) Otherwise, return a scaled version of the trajectory's theta.
 */
class PreferForwardCritic : public nav_dwb_controller_pkg::TrajectoryCritic
{
public:
  PreferForwardCritic()
  : penalty_(1.0), strafe_x_(0.1), strafe_theta_(0.2), theta_scale_(10.0) {}
  void onInit() override;
  double scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj) override;

private:
  double penalty_, strafe_x_, strafe_theta_, theta_scale_;
};

}  // namespace nav_dwb_critics
#endif  // NAV_DWB_CRITICS__PREFER_FORWARD_HPP_
